/* Copyright (C) 2011 Fredrique Samuels, fredriquesamuels@gmail.com 

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#include "collision_utils.h"

namespace tri{

void TSetBodyMatrix(NewtonBody* body, tri::TVector rot, tri::TVector pos){
	tri::TMatrix bodyLocation;
	tri::getRotationMatrixXYZ(rot.x, rot.y, rot.z,&bodyLocation);
	tri::translate(&bodyLocation, pos.x, pos.y, pos.z);
	NewtonBodySetMatrix (body, &bodyLocation.elements[0]);
}

NewtonBody* TCreateRigidBody (NewtonWorld* g_world, void* metaData, 
		NewtonCollision* shape, dFloat mass){
	dFloat origin[3];
	dFloat inertia[3];
	NewtonBody* body;
	
	body = NewtonCreateBody (g_world, shape,&mass);

	// bodies can have a destructor. 
	// this is a function callback that can be used to destroy any local data stored 
	// and that need to be destroyed before the body is destroyed. 
	NewtonBodySetDestructorCallback (body, TDestroyBodyCallback);

	// save the entity as the user data for this body
	NewtonBodySetUserData (body, metaData);

//	tri::TMatrix bodyLocation;
//	tri::translate(&bodyLocation,0,500,0);
//	NewtonBodySetMatrix (body, &bodyLocation.elements[0]);

	// we need to set the proper center of mass and inertia matrix for this body
	// the inertia matrix calculated by this function does not include the mass.
	// therefore it needs to be multiplied by the mass of the body before it is used.
	NewtonConvexCollisionCalculateInertialMatrix (shape, &inertia[0], &origin[0]);	

	// set the body mass matrix
	NewtonBodySetMassMatrix (body, mass, mass * inertia[0], mass * inertia[1], mass * inertia[2]);

	// set the body origin
	NewtonBodySetCentreOfMass (body, &origin[0]);

	// set the function callback to apply the external forces and torque to the body
	// the most common force is Gravity
	NewtonBodySetForceAndTorqueCallback (body, TApplyForceAndTorqueCallback);

	// set the function callback to set the transformation state of the graphic entity associated with this body 
	// each time the body change position and orientation in the physics world
	NewtonBodySetTransformCallback (body, TSetTransformCallback);
	
	NewtonReleaseCollision (g_world, shape);
	
	return body;
}

NewtonCollision* TCreateNewtonBox (NewtonWorld* g_world,tri::TVector size, int shapeId){
	tri::TMatrix matrix;
	tri::translate(&matrix,0,0,0);
	return NewtonCreateBox(g_world, size.x, size.y, size.z, shapeId, &matrix.elements[0]);
}

NewtonCollision* TCreateNewtonSphere (NewtonWorld* g_world,tri::TVector size, int shapeId){
	tri::TMatrix matrix;
	tri::translate(&matrix,0,0,0);
	return NewtonCreateSphere (g_world, size.x, size.y, size.z, shapeId, &matrix.elements[0]);
}

NewtonCollision* TCreateNewtonCylinder (NewtonWorld* g_world,tri::TVector size, int shapeId)
{
	tri::TMatrix matrix;
	tri::translate(&matrix,0,0,0);
	return NewtonCreateCylinder(g_world, size.x, size.y, shapeId, &matrix.elements[0]);
}

}

